# Sensor
# Receives, manipulates, and distributes sensor data
# Intended for use with sensor data from mobile devices 
# such as Nokia N95 or Android G1

import sys, socket
import time, threading
import collections
import copy
try:
    import cjson # preferred
except:
    try:
        import json # 2.6 but no cjson
    except:
        import simplejson as json # <2.6, at least simplejson
    class cjson(object):
        decode = staticmethod(json.loads)
        encode = staticmethod(json.dumps)
try:
    import bluetooth
except:
    bluetooth = None


IN_PORT       = 9617
OUT_HOST      = "localhost"
OUT_PORT      = 9642 
BLOCK_SIZE    = 17
UDP_PACK_SIZE = 1024


class Sensor(object):
    """Receive, manipulate, and distribute sensor values
    via UDP. Receives via UDP (in_port > 32) or bluetooth (in_port <=32)"""

    def __init__(self, in_port=IN_PORT, 
                 out_host=OUT_HOST, out_port=OUT_PORT):
        self.in_port = in_port
        self.out_host = out_host
        self.out_port = out_port
        self.receiver = None
        self.filters = []
        self.sinks = []
        self.rawsinks = []

    def start_receiving(self, in_port=None):
        "(re)start receiving on given or default port"
        if self.receiver:
            self.stop_receiving()
        if in_port:
            self.in_port = in_port
        if self.in_port < 32: # must be bluetooth
            # self.receiver = BluetoothReceiver(self.in_port)
            self.receiver = Receiver(BluetoothDataReceiver(self.in_port))
        else: # classical UDP
            # self.receiver = UDPReceiver(self.in_port)
            self.receiver = Receiver(UDPDataReceiver(self.in_port))
        self.receiver.register(self._update)
        self.receiver.start()

    def stop_receiving(self):
        "stop receiving if it is currently receiving"
        if self.receiver:
            self.receiver.shutdown()
            self.receiver = None

    def is_receiving(self):
        "is it currently receiving?"
        return self.receiver != None

    def register_filter(self, fil, pos=None):
        "appends a filtering function if not already present"
        if pos == None:
            self.filters.append(fil) # allow same filter many times
        else: # replace filter at position
            self.filters[pos] = fil

    def unregister_filter(self, fil):
        "removes first occurence of filtering function if present"
        if fil in self.filters: 
            self.filters.remove(fil)

    def register_sink(self, sink):
        "appends a final sink if not already present"
        if sink not in self.sinks:
            self.sinks.append(sink)
    def unregister_sink(self, sink):
        "removes a final sink if present"
        if sink in self.sinks:
            self.sinks.remove(sink)

    def register_rawsink(self, sink):
        "register a sink for raw data (before filtering)"
        if sink not in self.rawsinks:
            self.rawsinks.append(sink)
    def unregister_rawsink(self, sink):
        "unregister a sink for raw data (before filtering)"
        if sink in self.rawsinks:
            self.rawsinks.remove(sink)

    def reg_send(self, out_host=None, out_port=None):
        "Shortcut: Create and register sender to localhost"
        if out_host:
            self.out_host = out_host
        if out_port:
            self.out_port = out_port
        sender = UDPSender(self.out_host, self.out_port)
        self.register_sink(sender)
    def unreg_send(self):
        "Shortcut: Stop sending to host/port started with reg_send"
        for sink in self.sinks:
            if isinstance(sink, UDPSender):
                if sink.host == self.out_host:
                    if sink.port == self.out_port:
                        self.unregister_sink(sink)
                        sink.close()

    def reg_record(self, filename=None):
        "Shortcut: Start recording"
        recorder = Recorder(filename)
        self.register_rawsink(recorder)
    def unreg_record(self):
        "Shortcut: Stop recording"
        for sink in self.rawsinks:
            if isinstance(sink, Recorder):
                self.unregister_sink(sink)
                sink.close()

    def _update(self, block):
        """internal callback function processing filter chain and 
        sending to all sinks. Serial execution fine and on purpose."""
        for rsink in self.rawsinks:
            rsink(block[0])
        for fil in self.filters: # apply filter chain
            block = fil(block)
            if block == None: # filtering and distribution canceled
                return            
        for sink in self.sinks: # distribute result, first of block
            sink(block[0])


_initial_entry = {
    'x' : 0,
    'y' : 0,
    'z' : 0,
    'k' : "",
    'id' : -1
}
class Receiver(threading.Thread):
    "Receives and de-json's packets, manages and calls observers"

    def __init__(self, data_receiver):
        threading.Thread.__init__(self)
        self.data_receiver = data_receiver
        initial_block = []
        for _ in range(BLOCK_SIZE):
            initial_block.append(_initial_entry.copy())
        self.deque = collections.deque(initial_block)
        self.serving = True
        self.updaters = [] # must be list of callables; get a copy

    def register(self, func): 
        "registers callback function (one argument) on data arrival."
        # The callback function should be fast (sync call)
        if func not in self.updaters:
            self.updaters.append(func)

    def unregister(self, func):
        "unregisters callback function, if available"
        if func in self.updaters:
            self.updaters.remove(func)

    def run(self):
        "main loop, receiving packets and maintaining data"
        while self.serving:
            if not self.data_receiver.is_initialized():
                self.data_receiver.initialize()
            if self.data_receiver.is_initialized():
                data = self.data_receiver.receive()
                if data:
                    dic = cjson.decode(data) 
                    # update block
                    self.deque.rotate(1) # shift to make room on first
                    self.deque[0] = dic  # first is always new packet
                    if self.updaters:
                        for func in self.updaters:
                            block = copy.deepcopy(list(self.deque)) # safe copy
                            func(block)
        self.data_receiver.close()

    def shutdown(self):
        "Wish to shutdown at next convenient point in time"
        self.serving = False
        

class UDPDataReceiver(object):
    """DataReceiver for UDP packets"""
    
    def __init__(self, in_port):
        self.in_port = in_port
        self.server_socket = None
        
    def is_initialized(self):
        return self.server_socket != None

    def initialize(self):
        server_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
        server_socket.settimeout(0.5) # wait half a second
        server_socket.bind(('', self.in_port))
        self.server_socket = server_socket

    def receive(self):
        try:
            server_socket = self.server_socket
            if server_socket:
                data, _ = server_socket.recvfrom(UDP_PACK_SIZE)
                return data
            return None
        except socket.timeout: # may happen
            return None # None signifies nothing receives but ok

    def close(self):
        self.server_socket = None
        
        
class BluetoothDataReceiver(object):
    "DataReceiver for Bluetooth"    

    def __init__(self, in_port):
        if not bluetooth:
            print("bluetooth not supported")
            return
        self.in_port = in_port
        self.server_socket = None
        self.client_socket = None
        self.client_address = None

    def is_initialized(self):
        return self.server_socket != None and self.client_socket != None

    def initialize(self):
        if not bluetooth:
            return
        if self.server_socket == None:
            self.server_socket = bluetooth.BluetoothSocket(bluetooth.RFCOMM)
            try:
                self.server_socket.settimeout(5.0) # wait initially longer
            except:
                print("timeouts not supported, not good")
            self.server_socket.bind(('', self.in_port))
            self.server_socket.listen(1)
        try:
            cs, ca = self.server_socket.accept()
            self.client_socket, self.client_address = cs, ca
            # print("Accepted connection from ", self.client_address)
            try:
                self.server_socket.settimeout(1.0) # shorter while serving
            except:
                print("timeouts not supported, not good")
        except bluetooth.BluetoothError as data:
            # print("timeout?, do again:", data)
            pass

    def receive(self):
        try:
            data = self.client_socket.recv(UDP_PACK_SIZE)
            return data
        except bluetooth.BluetoothError as data:
            if "timed out" in data: # timeout
                return None # ignore timeouts, but ok so far
            else: # some other error
                print("bluetooth error", data)
                print("closing")
                self.close()

    def close(self):
        try:
            if self.client_socket:
                self.client_socket.close()
        except:
            print("closing bluetooth client failed")
        self.client_socket = None
        try:
            if self.server_socket:
                self.server_socket.close()
        except:
            print("closing bluetooth server failed")
        self.server_socket = None


class UDPSender(object):
    "send python data json'ed via UDP"

    def __init__(self, host, port):
        self.sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
        self.host = host
        self.port = port

    def __call__(self, packet):
        "just call the instance to send, makes it suitable as callback"
        jpacket = cjson.encode(packet)
        self.sock.sendto(jpacket + "\n", (self.host, self.port))

    def close(self):
        "proper shutdown"
        self.sock.close()


class Recorder(object):
    "records received data (raw) in a file"
    
    def __init__(self, filename=None):
        if filename == None:
            filename = time.strftime("%Y%m%d%H%M%S")+".sens"
        self.openfile = file(filename,"w")
        self.writing = True

    def __call__(self, packet):        
        packet = packet.copy()
        if "timestamp" not in packet:
            packet["timestamp"] = time.time()
        jpacket = cjson.encode(packet)
        try:
            self.openfile.write(jpacket+"\n")
        except: 
            if self.writing: # only if expected to work
                raise 

    def close(self):
        self.writing = False
        self.openfile.close()


class SensorReceiver(threading.Thread):
    """Class intended to receive final sensor input on localhost.
    Either register callback or sublass it and overwrite update method 
    to work with received python data. 
    Separate Thread, don't forget to call close.
    """

    def __init__(self, in_port=None):
        threading.Thread.__init__(self)
        self.host = "localhost"
        if in_port == None:
            in_port = OUT_PORT
        self.in_port = in_port
        self.server_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
        self.server_socket.settimeout(0.5) # wait half a second 
        self.server_socket.bind((self.host, self.in_port))
        self.serving = True
        self.updaters = []
        self.lock = threading.Lock()
        self.closed = False
        self.start() # automatically started!

    def run(self):
        "the main loop receiving packets"
        while self.serving:
            try:
                data, _ = self.server_socket.recvfrom(1024)
                data = data.strip()
                dic = cjson.decode(data)
                self.update(dic)
                self.lock.acquire()
                updaters = self.updaters[:]
                self.lock.release()
                for updater in updaters:
                    updater(dic)
            except: # should be socket.timeout, problem pygame?
                pass # ignore
            finally:
                if self.lock.locked():
                    self.lock.release()
        self.closed = True

    def close(self):
        "Do NOT forget to call close if you dont't need it any longer"
        self.lock.acquire()
        self.updaters = []
        self.lock.release()
        self.serving = False

    def close_wait(self):
        "close and wait until it finally closed"
        self.close()
        while not self.closed:
            time.sleep(0.01)

    def update(self, data):
        "called with a packet, may be overridden"
        pass

    def register(self, func, i=None):
        "register callback function expecting a dictionary for each packet"
        if i == None: # append
            self.lock.acquire()
            self.updaters.append(func) # same function several times ok
            self.lock.release()
        else: # replace at position i, if available
            self.lock.acquire()
            if len(self.updaters) < i:
                self.updaters[i] = func
            self.lock.release()

    def unregister(self, func):
        "unregister callback function"
        self.lock.acquire()
        if func in self.updaters:
            self.updaters.remove(func)
        self.lock.release()


